#include "board.h"


PID speed_pid;

/******************************************************************
 * 函 数 说 明：滤波器
 * 函 数 形 参：new_value:当前编码器计数值 last_value:上次编码器计数值
 * 函 数 返 回：
 * 作       者：XTHH
 * 备       注：
******************************************************************/
// 定义滤波器系数，这个值决定了滤波器的截止频率和滤波效果
#define ALPHA 0.1
float filtered_speed = 0.0; // 滤波后的速度值，初始设为0

// 低通滤波器函数
float lowpass_filter(float new_value, float last_value)
{
    return ALPHA * new_value + (1.0 - ALPHA) * last_value;
}

/******************************************************************
 * 函 数 说 明：定速pid计算模块
 * 函 数 形 参：target:目标速度 
 * 函 数 返 回：
 * 作       者：XTHH
 * 备       注：
******************************************************************/
//定速pid初始化
void speed_pid_init(void)
{
	//35 6 10
	//200, 10, 10,
    pid_init(&speed_pid, 35, 6, 10, 2000, 2000,50);
}

//定速pid数据读取
PID* get_speed_pid(void)
{
	return &speed_pid;
}

//定速pid的目标速度读取
int get_speed_pid_target(void)
{
	return speed_pid.target;
}

//定速pid
PID motor_velcity_control(int target, int get_encoder)
{
	//char buff[50]={0};
	int PWM;
	//当前速度
	filtered_speed = lowpass_filter(get_encoder, filtered_speed);
	//pid计算（结果为两轮速度）
	pid_calc(&speed_pid, target, filtered_speed);
	return speed_pid;
}

/******************************************************************
 * 函 数 说 明：定速pid调节模块
 * 函 数 形 参：pid_value:pid结构体 select:当前设置页选择的内容  add_or_subtract_flag：加/减设定
 * 函 数 返 回：
 * 作       者：XTHH
 * 备       注：
******************************************************************/
void set_speed_pid_parameter(PID* pid_value, int select, int add_or_subtract_flag)
{
	//加
	if( add_or_subtract_flag == 0 )
	{
		switch(select)
		{
			case 0://调整P
				pid_value->kp = pid_value->kp + 0.1;
				break;
			case 1://调整I
				pid_value->ki = pid_value->ki + 0.1;
				break;
			case 2://调整D
				pid_value->kd = pid_value->kd + 0.1;
				break;			
			case 3://调整target
				if( pid_value->target < 100 )
					pid_value->target = pid_value->target + 1;
				break;		
		}
	} 
	//减
	else 
	{
		switch(select)
		{
			case 0://调整P
				if(  pid_value->kp > 0 )
					pid_value->kp = pid_value->kp - 0.1;
				pid_value->kp = (pid_value->kp <= -0.0f) ? 0.0f : pid_value->kp;  // 消除负零
				break;
			case 1://调整I
				if( pid_value->ki > 0 )
					pid_value->ki = pid_value->ki - 0.1;
				pid_value->ki = (pid_value->ki <= -0.0f) ? 0.0f : pid_value->ki;  // 消除负零
				break;
			case 2://调整D
				if( pid_value->kd > 0 )
					pid_value->kd = pid_value->kd - 0.1;
				pid_value->kd = (pid_value->kd <= -0.0f) ? 0.0f : pid_value->kd;  // 消除负零
				break;			
			case 3://调整target
				if( pid_value->target > -100 )
					pid_value->target = (pid_value->target - 1);
				break;		
		}	
	}
}